#coding=utf-8
# 项目启动脚本
import os
import signal
import subprocess
import sys
import argparse
import asyncio
import websockets
import threading


def signal_handler(sig, frame):
    print("SLAM系统正在停止...")
    sys.exit(0)



# # WebSocket 服务器逻辑
# async def handle_connection(websocket, path):
#     print("WebSocket 连接已建立")
#     try:
#         async for message in websocket:
#             print(f"收到消息: {message}")
#             await websocket.send("Hello from WebSocket server")
#     except websockets.ConnectionClosed:
#         print("WebSocket 连接已关闭")

# def start_websocket_server():
#     print("启动 WebSocket 服务器...")
#     start_server = websockets.serve(handle_connection, "localhost", 8765)
#     asyncio.get_event_loop().run_until_complete(start_server)
#     asyncio.get_event_loop().run_forever()



def start_slam():

    # todo: 后面放开
    # if len(sys.argv) != 5:
    #     print("Usage: python main.py <yaml_file> <camYaml_file> <lafida_file> <data_file>")
    #     sys.exit(1)

    signal.signal(signal.SIGINT, signal_handler)

    # 读取命令行参数
    slam_executable = "./slam/MultiCol-SLAM/Examples/Lafida/multi_col_slam_lafida"  # 这是生成的SLAM可执行文件的路径
    
    # 测试lafida
    yaml_file = "/home/zc/slam/code/my-proj/slam/MultiCol-SLAM/Examples/small_orb_omni_voc_9_6.yml"  # 这是SLAM的字典文件
    camYaml_file = "/home/zc/slam/code/my-proj/slam/MultiCol-SLAM/Examples/Lafida/Slam_Settings_indoor1.yaml"  # 配置文件路径
    lafida_file = "/home/zc/slam/code/my-proj/slam/MultiCol-SLAM/Examples/Lafida/"
    data_file = "/home/zc/slam/data/lafida/outdoor_large_loop"  # 数据集路径

    # 测试myData
    # yaml_file = "/home/zc/slam/code/MultiCol-SLAM/MultiCol-SLAM/Examples/small_orb_omni_voc_9_6.yml"  # 这是SLAM的字典文件
    # camYaml_file = "/home/zc/slam/code/my-proj/slam/MultiCol-SLAM/Examples/Lafida/Slam_Settings_indoor1.yaml"  # 配置文件路径
    # lafida_file = "/home/zc/slam/code/my-proj/slam/MultiCol-SLAM/Examples/Lafida/"
    # data_file = "/home/zc/slam/data/myData/outdoor/"  # 数据集路径
    
    # data_file = "/home/zc/slam/data/myData/outdoor"  # 数据集路径
    # slam_executable = "./slam/MultiCol-SLAM/Examples/Lafida/multi_col_slam_lafida"  # 这是生成的SLAM可执行文件的路径
    
    # 实际使用放开
    # yaml_file = sys.argv[1]  # 这是SLAM的字典文件
    # camYaml_file = sys.argv[2]  # 配置文件路径
    # lafida_file = sys.argv[3]
    # data_file = sys.argv[4]  # 数据集路径


    # 启动SLAM系统
    try:
        # subprocess.call([slam_executable, yaml_file, camYaml_file, lafida_file, data_file])
        process = subprocess.Popen([slam_executable, yaml_file, camYaml_file, lafida_file, data_file])
        pid = process.pid
        print("SLAM系统已启动，进程ID为：", pid)
        # 将 PID 写入文件
        with open('slam_pid.txt', 'w') as f:
            f.write(str(pid))
    except Exception as e:
        print("SLAM系统启动失败：", str(e))

    print("嘿！starting SLAM...")

if __name__ == '__main__':

    # # 启动 WebSocket 服务器
    # websocket_thread = threading.Thread(target=start_websocket_server)
    # websocket_thread.daemon = True  # 设置为守护线程，主线程退出时自动退出
    # websocket_thread.start()

    start_slam()